#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include <pluginlib/class_loader.hpp>

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/planning_scene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <sensor_msgs/msg/joint_state.hpp>
#include <bl_robot/msg/arm_goal_state.hpp>

using std::placeholders::_1;
namespace rvt = rviz_visual_tools;

class BlRobotCtrl : public rclcpp::Node
{
  public:
    const std::string PLANNING_GROUP = "the_arm";
    BlRobotCtrl()
    : Node("BlRobot")
    {
      subscription_ = this->create_subscription<bl_robot::msg::ArmGoalState>(
        "BlRobotCtrl", 10, std::bind(&BlRobotCtrl::topic_callback, this, _1));
      jointStatePublisher_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
      const std::shared_ptr<Node> robot_node= std::shared_ptr<Node>(this);
      robot_model_loader::RobotModelLoader robot_model_loader( robot_node, "robot_description");
      robot_model_ptr = robot_model_loader.getModel();
      
      robot_statePtr = moveit::core::RobotStatePtr(new moveit::core::RobotState(robot_model_ptr));
      joint_model_group = robot_statePtr->getJointModelGroup(PLANNING_GROUP);
  
      planning_scene= planning_scene::PlanningScenePtr(new planning_scene::PlanningScene(robot_model_ptr));
      planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
      
      joint_names = joint_model_group->getJointModelNames();
      
      std::vector<double> joint_values;
      RCLCPP_INFO(this->get_logger(), "Print joint values, start ...");
      //planning_scene->getCurrentState().printStatePositions();
      planning_scene->getCurrentState().copyJointGroupPositions( joint_model_group, joint_values);
  
     for (std::size_t i = 0; i < joint_names.size(); ++i)
     {
        RCLCPP_INFO(this->get_logger(), "Joint %s: %d / %f", joint_names[i].c_str(), 
        	(int)(joint_values[i]*180/3.1416),joint_values[i] );
     }
     RCLCPP_INFO(this->get_logger(), "Print joint values, end.");
     /*
      const std::shared_ptr<rclcpp::Publisher<std_msgs::msg::String>> debugPublisher_ 
              =  this->create_publisher<std_msgs::msg::String>("BlRobotCtrl", 10);
      auto message = std_msgs::msg::String();
      message.data = "ready to publish! ";
      debugPublisher_ -> publish(message);*/
      
      //if (!this->get_parameter("planning_plugin", planner_plugin_name))
    	//RCLCPP_FATAL(this->get_logger(), "Could not find planner plugin name");
     try
     {
        planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
            "moveit_core", "planning_interface::PlannerManager"));
      }
      catch (pluginlib::PluginlibException& ex)
      {
          RCLCPP_FATAL(this->get_logger(), "Exception while creating planning plugin loader %s", ex.what());
     }
     
     rclcpp::Node::SharedPtr thisNodePtrTmp = this->shared_from_this();
     const rclcpp::Node::SharedPtr& thisNodePtr = thisNodePtrTmp;
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model_ptr, thisNodePtr,this->get_namespace()))
      RCLCPP_FATAL(this->get_logger(), "Could not initialize planner instance");
    RCLCPP_INFO(this->get_logger(), "Using planning interface '%s'", planner_instance->getDescription().c_str());
  }
  catch (pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (const auto& cls : classes)
      ss << cls << " ";
    RCLCPP_ERROR(this->get_logger(), "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_plugin_name.c_str(),
                 ex.what(), ss.str().c_str());
  }
  	display_publisher =
           this->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path",1);
           
     move_groupPtr = new moveit::planning_interface::MoveGroupInterface( thisNodePtr, PLANNING_GROUP);    
    //move_groupPtr =  std::shared_ptr<moveit_visual_tools::MoveItVisualTools>(moveGroupPtrTmp);// std::shared_moveGroupPtr->shared_from_this();
   //namespace rvt = rviz_visual_tools;  
   moveit_visual_tools::MoveItVisualTools* visual_toolsPtr = new moveit_visual_tools::MoveItVisualTools( thisNodePtr, "base_link", "the_arm", move_groupPtr->getRobotModel());
  
     //visual_toolsPtr = std::make_shared<moveit_visual_tools::MoveItVisualTools>( visualTool );
  visual_toolsPtr->enableBatchPublishing();
  visual_toolsPtr->deleteAllMarkers();
  visual_toolsPtr->trigger();

  /* Remote control is an introspection tool that allows users to step through a high level script
     via buttons and keyboard shortcuts in RViz */
  visual_toolsPtr->loadRemoteControl();

  /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_toolsPtr->publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);

  /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */
  visual_toolsPtr->trigger();
     
    }
//////////////////////////////////////////////////////////////////////////////////////
  private:
    void topic_callback(const bl_robot::msg::ArmGoalState & msg) 
    {
      RCLCPP_INFO(this->get_logger(), "cmd: %s , id: %s, goal_type: %s", msg.cmd.c_str(), msg.id.c_str(), msg.goal_type.c_str());
      //RCLCPP_INFO(this->get_logger())
      
      geometry_msgs::msg::PoseStamped pose;
      if( msg.goal_type=="W" ){
      	  pose.header.frame_id = "base_link";
	  pose.pose.position.x = msg.position[0];
	  pose.pose.position.y = msg.position[1];
	  pose.pose.position.z = msg.position[2];
	  pose.pose.orientation.x = msg.position[3];
	  pose.pose.orientation.y = msg.position[4];
	  pose.pose.orientation.z = msg.position[5];
	  pose.pose.orientation.w = msg.position[6];
      }else{
      
      }
      
      planning_interface::MotionPlanRequest req;
      planning_interface::MotionPlanResponse res;
  
      std::vector<double> tolerance_pose(3, 0.01);
      std::vector<double> tolerance_angle(3, 0.01);
      
      moveit_msgs::msg::Constraints pose_goal =
      kinematic_constraints::constructGoalConstraints("link6", pose, tolerance_pose, tolerance_angle);

      req.group_name = PLANNING_GROUP;
      req.goal_constraints.push_back(pose_goal);
      
      planning_interface::PlanningContextPtr context =
      planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  if (res.error_code_.val != res.error_code_.SUCCESS)
  {
    RCLCPP_ERROR(this->get_logger(), "Could not compute plan successfully");
    //return 0;
  }
  
  // Visualize the result
  // ^^^^^^^^^^^^^^^^^^^^
  
      //this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
  //moveit_msgs::msg::DisplayTrajectory display_trajectory;

  /* Visualize the trajectory */
  moveit_msgs::msg::MotionPlanResponse response;
  res.getMessage(response);
  
  std::vector<double> joint_valuesYY;
  res.trajectory_->getLastWayPoint().copyJointGroupPositions( joint_model_group, joint_valuesYY);

  this->display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  visual_toolsPtr->publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_toolsPtr->trigger();
  display_publisher->publish(display_trajectory);

  /* Set the state in the planning scene to the final state of the last plan */
  robot_statePtr->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState( *robot_statePtr.get() );

  // Display the goal state
  visual_toolsPtr->publishAxisLabeled(pose.pose, "goal_1");
  static Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  visual_toolsPtr->publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
  visual_toolsPtr->trigger();
  
  RCLCPP_INFO(this->get_logger(), "Print current joint values, [1]");
  //planning_scene->getCurrentState().printStatePositions();
  std::vector<double> joint_values;
  robot_statePtr->copyJointGroupPositions( joint_model_group, joint_values);
  
  auto message2 = std_msgs::msg::String();
  //char[] msgData = 
  message2.data="data:[";
  
  sensor_msgs::msg::JointState endJointState;
  	int nJointCount =  joint_names.size();
  	rclcpp::Clock theClock;
  	endJointState.header.stamp = theClock.now();
  	endJointState.name.resize(nJointCount) ;
  	endJointState.position.resize(nJointCount);//= 
  	
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(this->get_logger(), "Joint %s: %d / %f", joint_names[i].c_str(), (int)(1000*joint_values[i]*180/3.1416) , joint_values[i] );
    if( i>0 ){
    	message2.data +=",";
    }
  	message2.data += joint_names[i] + ":"+ std::to_string( (int)(1000*joint_values[i]*180/3.1416) ) ;
  	
  	endJointState.name[i] = joint_names[i];
  	endJointState.position[i] = joint_values[i];
  }
  message2.data+="]";
  
  	//publisher_ -> publish(message2);
  	
  	
  	jointStatePublisher_ -> publish(endJointState);
  
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(this->get_logger(), "Joint %s: %d", joint_names[i].c_str(), (int)(joint_valuesYY[i]*180/3.1416) );
  }
  
  RCLCPP_INFO(this->get_logger(), "Print Current joint values, [1]");
  
      //char firstChar = msg.data.c_str()[0];
    }
    /*
    void getPose(const bl_robot::msg::ArmGoalState & msg, geometry_msgs::msg::PoseStamped& posePtr){
    	//geometry_msgs::msg::PoseStamped pose;
	  posePtr.header.frame_id = "base_link";
	  posePtr.pose.position.x = msg.position[0];
	  posePtr.pose.position.y = msg.position[1];
	  posePtr.pose.position.z = msg.position[2];
	  posePtr.pose.orientation.w = msg.position[3];
	  posePtr.pose.orientation.w = msg.position[4];
	  posePtr.pose.orientation.w = msg.position[5];
	  posePtr.pose.orientation.w = msg.position[6];
	  //return pose;
    }	*/
    
    //rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
    rclcpp::Subscription<bl_robot::msg::ArmGoalState>::SharedPtr subscription_;
    std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> jointStatePublisher_;
    moveit::core::RobotModelPtr robot_model_ptr;
    
    moveit::core::RobotStatePtr robot_statePtr;
    const moveit::core::JointModelGroup* joint_model_group;
    planning_scene::PlanningScenePtr planning_scene;
    
    std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
  planning_interface::PlannerManagerPtr planner_instance;
  std::string planner_plugin_name = "ompl_interface/OMPLPlanner";
    
    std::vector<std::string> joint_names;
    
    moveit_msgs::msg::DisplayTrajectory display_trajectory;
    rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_publisher;
    //std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_toolsPtr;
    moveit_visual_tools::MoveItVisualTools* visual_toolsPtr;
    moveit::planning_interface::MoveGroupInterface* move_groupPtr;
    //std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_groupPtr;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<BlRobotCtrl>());
  rclcpp::shutdown();
  return 0;
}
